package com.wind.util;

import Jama.Matrix;

/**
 * Created by jiaquanzhen on 2018-09-05.
 */
public class JamaTest {

    public static Matrix  MatrixACrossProductMatrixB(Matrix uNOi,Matrix Hi,String sign)
    {
        Matrix  Zi2i = null;
        double a1,a2,a3,b1,b2,b3,c1= 0,c2 = 0,c3= 0;
        if(sign.equals("-")) {
            a1=uNOi.get(0,0);
            a2=uNOi.get(0,1);
            a3=uNOi.get(0,2);
            b1= -Hi.get(0,0);
            b2= -Hi.get(0,1);
            b3= -Hi.get(0,2);
            c1=a2*b3-a3*b2;
            c2=-(a1*b3-a3*b1);
            c3=a1*b2-a2*b1;


        }
        double [][]  zzz={{c1,c2,c3}};
        Zi2i = new Matrix(zzz);
        return  Zi2i;
    }
    public static double  MatrixADotProductMatrixB(Matrix uNOi,Matrix Hi )
    {

        double a1,a2,a3,b1,b2,b3,c1= 0,c2 = 0,c3= 0;

            a1=uNOi.get(0,0);
            a2=uNOi.get(0,1);
            a3=uNOi.get(0,2);
            b1= Hi.get(0,0);
            b2= Hi.get(0,1);
            b3= Hi.get(0,2);
            c1=a1*b1+a2*b2+a3*b3;

        return  c1;
    }

    public static Matrix  MatrixAnormMatrixB(Matrix Ri)
    {
        double nRi = Ri.normF();
        double [][] arrayccnRi= {{nRi,nRi,nRi}};
        Matrix  BnRi = new Matrix(arrayccnRi);
        Matrix  uRi= BnRi.arrayLeftDivide(Ri);
        return  uRi;
    }

    public static double [][]  MatrixJ2000ToInertial(Matrix Ri,Matrix Vi)
    {
//        double nRi = Ri.normF();
//        double nVi = Vi.normF();
//
//        double [][] arrayccnRi= {{nRi,nRi,nRi}};
//        Matrix  BnRi = new Matrix(arrayccnRi);
//
//        double [][] arrayccnVi= {{nVi,nVi,nVi}};
//        Matrix  BnVi = new Matrix(arrayccnVi);
//
//        Matrix  uRi= BnRi.arrayLeftDivide(Ri);
//        Matrix  uVi= BnVi.arrayLeftDivide(Vi);


//        Hi = [ uRi(2,1)*uVi(3,1) - uRi(3,1)*uVi(2,1) ; ...,
//        uRi(3,1)*uVi(1,1) - uRi(1,1)*uVi(3,1) ; ...,
//        uRi(1,1)*uVi(2,1) - uRi(2,1)*uVi(1,1) ] ;
//

        Matrix uRi= MatrixAnormMatrixB(Ri);
        Matrix uVi=MatrixAnormMatrixB(Vi);

//        double tes= uRi.get(2,1);
//        double tess= uRi.get(1,1);
        double [][] Hi = {{ uRi.get(0,1)*uVi.get(0,2)-uRi.get(0,2)*uVi.get(0,1),
        uRi.get(0,2)*uVi.get(0,0)-uRi.get(0,0)*uVi.get(0,2),
        uRi.get(0,0)*uVi.get(0,1)-uRi.get(0,1)*uVi.get(0,0)}};

        double [][] Zi ={{0,0,1}};

        double [][] NOi = {{ uRi.get(0,2)*uVi.get(0,0) - uRi.get(0,0)*uVi.get(0,2),
                uRi.get(0,2)*uVi.get(0,1) - uRi.get(0,1)*uVi.get(0,2),0}};

        Matrix  BnRitemp = new Matrix(NOi);

//        double nVitemp = BnRitemp.normF();
//        double [][] arrayccnVitemp= {{nVitemp,nVitemp,nVitemp}};
//        Matrix  itemp = new Matrix(arrayccnVitemp);
//       Matrix uNOi = itemp.arrayLeftDivide(BnRitemp);

        Matrix uNOi  =MatrixAnormMatrixB(BnRitemp);

        Matrix Zi2i = MatrixACrossProductMatrixB(uNOi,new Matrix(Hi),"-");

        Matrix uZi2i =MatrixAnormMatrixB(Zi2i);
        double [][]  Ai2i={{uNOi.get(0,0) , uNOi.get(0,1) , uNOi.get(0,2)},
                {-Hi[0][0] , -Hi[0][1] , -Hi[0][2]},
                {uZi2i.get(0,0) , uZi2i.get(0,1) , uZi2i.get(0,2)}};

        return Ai2i;

    }
    public static void main(String[] args) {

        double asdfd =-0.99115;
        double dasdf= Math.acos(asdfd);

        double dasddf=  Math.toDegrees(dasdf);

        double dasddfd= Math.cos(dasdf);
        double dddds= Math.sin(dasdf);


        double dasddfxxd= Math.cos(Math.toRadians (dasddf));
        double ddddxxs= Math.sin(Math.toRadians (dasddf));

        int fdfd =4;
        double ddd = Math.pow(fdfd,3);
//        double dddd = Math.pow(fdfd,0.5);
//        double ddddd = fdfd^3;
//                double [][] arraybccs = {
//                {-1,1,0},
//                {-4,3,0},
//                {1 ,0,2}};
//
//        //定义一个矩阵
//        Matrix Acc = new Matrix(arraybccs);
//
//
//        double [][] tttHi = {{0.7506,-0.6483,0.1276 }};
//
//        Matrix Abcc =new Matrix(tttHi);
//
//        Matrix ddf = Abcc.times(Acc);
//
//        double [][] Hi = {{0.7506,-0.6483,0.1276 }};
//
//        double [][] uNOi = {{0.6536,0.7568,0 }};
//
//        Matrix tdd = MatrixACrossProductMatrixB(new Matrix(uNOi),new Matrix(Hi),"-");
//
//        Matrix uNOic =new Matrix(uNOi).times(new Matrix(Hi).transpose() );
//
//        Matrix uNOit =new Matrix(uNOi).arrayTimesEquals(new Matrix(Hi));

        int id=1;
//        double [][] array = {
//                {-1,1,0},
//                {-4,3,0},
//                {1 ,0,2}};
//        //定义一个矩阵
//        Matrix A = new Matrix(array);
//
//        //由特征值组成的对角矩阵
//        A.eig().getD().print(4,2);
//        //每一列对应的是一个特征向量
//        A.eig().getV().print(4,2);

//        double [][] array = {
//                {-4496.623237,-5204.717682,7.867862}};
//        Matrix A = new Matrix(array);
//        double c = A.norm1();
//        double cb = A.norm2();
//        double nf = A.normF();
//        double t =A.normInf();
//
//        double [][] arraycc= {{nf,nf,nf}};
//        Matrix  B = new Matrix(arraycc);
//        Matrix  CC= B.arrayLeftDivide(A);
//        double [][] arrayccff= A.getArray();
//        int i=8;
//        i=8;

        double [][] array = {
                {-4496.623237,-5204.717682,7.867862}};
        Matrix A = new Matrix(array);
        double c = A.norm1();
        double cb = A.norm2();
        double nf = A.normF();
        double t =A.normInf();

        double [][] arraycc= {{nf,nf,nf}};
        Matrix  B = new Matrix(arraycc);
        Matrix  CC= B.arrayLeftDivide(A);
        double [][] arrayccff= A.getArray();
        int i=8;
        i=8;
    }
}
